#include "rclcpp/rclcpp.hpp"    // ROS 2 C++客户端库核心头文件   
#include "geometry_msgs/msg/twist.hpp" // ROS 几何消息类型（非时间戳速度指令）
#include "geometry_msgs/msg/twist_stamped.hpp" // 带时间戳的速度指令
#include <thread>// 多线程支持
#include <map> // STL映射容器
#include <termios.h> // 终端I/O控制
#include <unistd.h>// POSIX标准函数（如getchar）
#include <sys/select.h>// I/O多路复用

using namespace std::chrono_literals; // 时间字面量（如1s）

// // 跨平台键盘输入处理（非阻塞读取）
int getch() {
    // 定义新旧终端属性结构体
    struct termios oldt, newt;
    int ch;// 存储读取的字符
    int fd = STDIN_FILENO;// 获取标准输入的文件描述符（值为0）
    /* 1. 保存当前终端设置 */
    tcgetattr(fd, &oldt);// 获取当前终端属性,以便后续恢复
     /* 2. 配置新终端模式 */
    newt = oldt;// 复制旧属性到新结构体（保留其他未修改的设置）
    newt.c_lflag &= ~(ICANON | ECHO);// ICANON：禁用规范模式和ECHO :禁止输入字符回显到屏幕
        /* 3. 应用新设置 */
    // 立即生效新配置（TCSANOW表示立即应用）
    tcsetattr(fd, TCSANOW, &newt);// 设置新属性
    /* 4. 读取字符 */
    // 此时终端处于非阻塞模式，无需回车即可读取单个字符
    ch = getchar();// 读取单个字符
    /* 5. 恢复原始设置 */
    // 将终端属性还原为之前保存的状态
    tcsetattr(fd, TCSANOW, &oldt);// 恢复原属性
    return ch;
}

class RobotControl : public rclcpp::Node {
public:
    // 构造函数继承自ROS 2节点，设置节点名为"myrobot_control",节点名称是ROS图（Graph）中的唯一标识符
    RobotControl() : Node("myrobot_control") {
        //  // 声明参数（是否使用时间戳，帧ID） 
        stamped_ = this->declare_parameter<bool>("stamped", false);
        frame_id_ = this->declare_parameter<std::string>("frame_id", "");
        
         // 参数有效性验证（当stamped为false时禁止设置frame_id）
        //验证逻辑：当同时满足以下条件时抛出异常：
        //未启用时间戳（!stamped_）//指定了非空坐标系（!frame_id_.empty()）
        if (!stamped_ && !frame_id_.empty()) {
            throw std::runtime_error("'frame_id' can only be set when 'stamped' is true");
        }

         // 根据参数创建对应类型的发布者
        if (stamped_) {
            stamped_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("myrobot_control", 10);
            stamped_msg_.header.frame_id = frame_id_;// 设置帧ID
        } else {
            twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("myrobot_control", 10);
        }

      // 初始化运动参数
        speed_ = 0.5;// 默认线速度系数
        turn_ = 1.0;// 默认角速度系数
        x_=0, y_=0, z_=0, th_=0;// 各方向速度分量
    }

    void run() {
        // 打印控制说明
const char* msg = R"(

Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

)";
        std::cout << msg << std::endl;

        // 主循环（ROS运行期间持续执行）
        while (rclcpp::ok()) {
            int key = getch();// 获取按键
            processKey(key); // 处理按键逻辑
            publishVelocity();// 发布速度指令
            rclcpp::spin_some(shared_from_this());// 处理ROS事件
        }
    }

private:
    void processKey(int key) {
        // // 运动方向映射表（键值对应各方向分量）
        static const std::map<int, std::vector<double>> move_bindings = {
            {'i', {1, 0, 0, 0}},   // 前进
            {'o', {1, 0, 0, -1}},  // 右转前进
            {'j', {0, 0, 0, 1}},   // 左转
            {'l', {0, 0, 0, -1}},  // 
            {'u', {1, 0, 0, 1}},
            {',', {-1, 0, 0, 0}},
            {'.', {-1, 0, 0, 1}},
            {'m', {-1, 0, 0, -1}},
            {'O', {1, -1, 0, 0}},
            {'I', {1, 0, 0, 0}},
            {'J', {0, 1, 0, 0}},
            {'L', {0, -1, 0, 0}},
            {'U', {1, 1, 0, 0}},
            {'<', {-1, 0, 0, 0}},
            {'>', {-1, -1, 0, 0}},
            {'M', {-1, 1, 0, 0}},
            {'t', {0, 0, 1, 0}},//上升
            {'b', {0, 0, -1, 0}},//下降

        };

        //  // 速度调整映射表（q/z调整整体，w/x调整线速度，e/c调整角速度）
        static const std::map<int, std::vector<double>> speed_bindings = {
            {'q', {1.1, 1.1}},// 增加10%
            {'z', {0.9, 0.9}},// 减少10%
            {'w', {1.1, 1}},
            {'x', {0.9, 1}},
            {'e', {1, 1.1}},
            {'c', {1, 0.9}},
            
        };

        if (move_bindings.count(key)) {
            const auto& cmd = move_bindings.at(key);
            x_ = cmd[0]; // 更新线速度x分量
            y_ = cmd[1]; // 更新线速度y分量
            z_ = cmd[2]; // 更新线速度z分量
            th_ = cmd[3];// 更新角速度z分量
        } 
        else if (speed_bindings.count(key)) {
            const auto& factor = speed_bindings.at(key);
            speed_ *= factor[0];// 调整线速度系数
            turn_ *= factor[1];// 调整角速度系数
            std::cout << "\rCurrent: speed " << speed_ << "\tturn " << turn_ << std::flush;
        } 
         // 处理Ctrl-C
        else if (key == 3) { // Ctrl+C 
            stopRobot();// 发送停止指令
            rclcpp::shutdown();// 关闭ROS
        } else {
            x_ = y_ = z_ = th_ = 0;// 无效按键停止运动
        }
    }

    void publishVelocity() {
        // // 更新时间戳并发布带时间戳的消息
        if (stamped_) {
            stamped_msg_.header.stamp = now();// 设置当前时间戳
            geometry_msgs::msg::Twist& twist = stamped_msg_.twist;// 填充速度数据
            updateTwistFields(twist);
            stamped_pub_->publish(stamped_msg_);
        } 
            // 发布普通消息
        else {
            geometry_msgs::msg::Twist twist;
            updateTwistFields(twist);
            twist_pub_->publish(twist);
        }
    }
    // 填充Twist消息字段
    void updateTwistFields(geometry_msgs::msg::Twist& twist) {
        twist.linear.x = x_ * speed_;// 计算实际线速度x分量
        twist.linear.y = y_ * speed_; // 线速度y分量
        twist.linear.z = z_ * speed_;// 垂直方向速度
        twist.angular.x = 0.0;
        twist.angular.y = 0.0;
        twist.angular.z = th_ * turn_;// 角速度
    }

     // 紧急停止方法
    void stopRobot() {
        // 发布零速度 
        if (stamped_) {
            stamped_msg_.twist = geometry_msgs::msg::Twist();// 清零所有速度
            stamped_pub_->publish(stamped_msg_);
        } else {
            auto twist = geometry_msgs::msg::Twist();// 创建空消息
            twist_pub_->publish(twist);
        }
    }

    // 成员变量
    bool stamped_;// 是否使用时间戳格式
    std::string frame_id_;// 坐标系ID
    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr stamped_pub_;// 时间戳发布器
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_; // 普通发布器
    geometry_msgs::msg::TwistStamped stamped_msg_;// 缓存带时间戳的消息
    double speed_, turn_; // 速度系数
    double x_=0, y_=0, z_=0, th_=0;// 当前速度分量
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);// 初始化ROS 2
    auto node = std::make_shared<RobotControl>();// 创建节点实例
    node->run();// 启动控制循环
    return 0;
}